package com.rockwell.robot2015.model.states.running.sequence;

import org.opencv.core.Point;

import com.rockwell.robot2015.model.Robot;
import com.rockwell.robot2015.model.RobotAction;
import com.rockwell.robot2015.model.detectors.IDetector;
import com.rockwell.robot2015.model.sensors.IRobotSensors;
import com.rockwell.robot2015.model.states.IState;
import com.rockwell.robot2015.model.states.IStateMachine;
import com.rockwell.robot2015.model.states.running.RobotRunningStateMachine;

public class FindingMazeEntranceSequence extends RobotRunningSequence {
	/**
	 * Alignment threshold beyond which we should adjust the orientation of the robot.
	 */
	private final static double ALIGNMENT_THRESHOLD = 5;

	private final static int CORNER_CLEARANCE1 = 17;
	
	private final static int CORNER_CLEARANCE2 = 33;
	
	/**
	 * The distance where we should stop jogging and start indexing
	 */
	private final static double INDEX_DISTANCE = 60; // %

	/**
	 * small indexing distance
	 */
	public static int SMALL_MOVE_INDEX = 2; // mm
	
	/**
	 * Big indexing distance
	 */
	public static int BIG_MOVE_INDEX = 8; // mm

	
	enum State implements IState {
		Start {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				machine.robot().log("Start finding maze entrance", 2);

				machine.robot().getSensor().setInMaze(false);

				machine.changeState(Jog);
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		}, 
		Detect {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				IDetector detector = robot.getMazeEntranceGuideDetector();
				
				if (detector.isPresent()) {
					Point[] line = detector.getLine();
					
					if (line[0] != null && line[1] != null) {
						double dx = line[0].x - line[1].x;
						double dy = line[0].y - line[1].y;
						
						int degree = (int)Math.toDegrees(Math.atan2(dx, dy));
						degree /= 2;
						
						String log = String.format("Turning degree: %d", degree);
						robot.log(log, 4);

						if (Math.abs(degree) > ALIGNMENT_THRESHOLD) {
							robot.sendCommand(RobotAction.TurnDegree, degree);
							robot.sleep(Robot.MOTION_SETTLING_TIME);
						}
						else {
							machine.changeState(ApproachEntrance);
						}
					}
					else {
						machine.changeState(ApproachEntrance);
					}
				}
				else {
					machine.changeState(Jog);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		Jog {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("FindMazeEntrance: Jog", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				IRobotSensors sensor = robot.getSensor();
				
				if (sensor.detectLeftWall()) {
					robot.sendCommand(RobotAction.Jog);

					if (sensor.detectFrontWall()) {
						/* If it is at the corner, move forward a bit and turn right.
						 *  
						 */
						robot.sendCommand(RobotAction.TurnRight);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						//robot.startJogging();
					}
					else {
						machine.changeState(CornerOrEntranceReached);
					}
				}
				else {
					machine.changeState(MazeLost);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		CornerOrEntranceReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Corner or entrance reached", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				machine.robot().log("Move for corner clearance", 4);
				
				IRobotSensors sensor = robot.getSensor();
				
				boolean hasLeftWall = sensor.detectLeftWall();
				boolean hasRightWall = sensor.detectRightWall();
				boolean hasFrontWall = sensor.detectFrontWall();
				
				String log = "LeftSensor: " + hasLeftWall + ", RightSensor: " + hasRightWall + ", FrontSensor: " + hasFrontWall;
				robot.log(log, 4);
				
				robot.sendCommand(RobotAction.MoveWithSensor, CORNER_CLEARANCE1);
				robot.sleep(Robot.MOTION_SETTLING_TIME);

				if (!hasLeftWall) {
					if (hasFrontWall && !hasRightWall) {
						// There a wall in front, but not on the left and right side, 
						// and continue following the wall.
						machine.robot().log("Turn Right", 4);
						
						robot.turnRight();
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					}
					else {
						machine.robot().log("Turn left", 4);
					
						robot.turnLeft();
						robot.sleep(Robot.MOTION_SETTLING_TIME);
						
						robot.sendCommand(RobotAction.MoveWithSensor, CORNER_CLEARANCE2);
						robot.sleep(Robot.MOTION_SETTLING_TIME);

						robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);

						hasLeftWall = sensor.detectLeftWall();
						hasRightWall = sensor.detectRightWall();
						hasFrontWall = sensor.detectFrontWall();
						
						log = "LeftSensor: " + hasLeftWall + ", RightSensor: " + hasRightWall + ", FrontSensor: " + hasFrontWall;
						robot.log(log, 4);
						
						if (hasLeftWall && hasRightWall) {
							machine.changeState(EntranceReached);
							return;
						}
						else if (hasLeftWall) {
							machine.changeState(Jog);
							return;
						}
						else {
							/*
							 * No side walls. detect front wall
							 */
							machine.robot().log("No sides walls after turning", 4);
							
							robot.sendCommand(RobotAction.MoveWithSensor, CORNER_CLEARANCE2);
							robot.sleep(Robot.MOTION_SETTLING_TIME);

							//robot.turnLeft();
							//robot.sleep(Robot.MOTION_SETTLING_TIME);

							hasFrontWall = sensor.detectFrontWall();
							hasLeftWall = sensor.detectLeftWall();
							hasRightWall = sensor.detectRightWall();

							log = "LeftSensor: " + hasLeftWall + ", RightSensor: " + hasRightWall + ", FrontSensor: " + hasFrontWall;
							robot.log(log, 4);
							
							if (hasLeftWall && hasRightWall) {
								machine.changeState(EntranceReached);
								return;
							}
							else if (hasLeftWall) {
								robot.adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
								machine.changeState(Jog);
								return;
							}
							else {
								// no left wall
								machine.changeState(MazeLost);
							}
						}
					}

				}
				else if (!hasFrontWall) {
					machine.changeState(Jog);
					return;
				}
				else if (!hasRightWall) {
					machine.robot().log("Turn Right", 4);
					
					robot.turnRight();
					robot.sleep(Robot.MOTION_SETTLING_TIME);
				}
				else {
					// dead end. should never happen. turn back anyway
					robot.turnBack();
					robot.sleep(Robot.MOTION_SETTLING_TIME);
					return;
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		ApproachEntrance {

			@Override
			public void enter(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void move(IStateMachine machine) {
				Robot robot = machine.robot();
				
				IDetector detector = robot.getMazeEntranceGuideDetector();
				
				if (detector.isPresent()) {
					/*
					double distance = detector.getDistanceInPercentage();
					
					robot.log(String.format("Treasure distance in percentage: %f", distance), 3);
					
					if (distance < PICK_DISTANCE) {
						// it is close enough to the treasure.
						machine.changeState(EntranceReached);
					}
					else {
						int step;
						
						if (distance > INDEX_DISTANCE) {
							// it is still far away, move a big step.
							step = BIG_MOVE_INDEX;
						}
						else {
							// It is close but not close enough yet.
							step = SMALL_MOVE_INDEX;
						}
						
						robot.log(String.format("Move %d", step), 3);
						
						robot.sendCommand(RobotAction.Move, step);
						robot.sleep(Robot.MOTION_SETTLING_TIME);
					}
					*/
				}
				else {
					machine.changeState(EntranceReached);
				}
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		EntranceReached {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Entrance reached", 2);

				machine.robot().adjustAngle(Robot.ADJUST_ANGLE_THRESHOLD);
				
				machine.robot().getSensor().setInMaze(true);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		},
		MazeLost {

			@Override
			public void enter(IStateMachine machine) {
				machine.robot().log("Maze lost", 2);
			}

			@Override
			public void move(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}

			@Override
			public void exit(IStateMachine machine) {
				// TODO Auto-generated method stub
				
			}
			
		};
		
		static FindingMazeEntranceSequence m_sequence;
		
		public static void setSequence(FindingMazeEntranceSequence sequence) {
			m_sequence = sequence;
		}
		
	}
	
	public FindingMazeEntranceSequence(RobotRunningStateMachine running) {
		super(running, State.Start);
		
		State.setSequence(this);
		
		m_currState = State.Start;
	}

	@Override
	public void start() {
		m_currState = State.Start;
		m_nextState = null;
		
		Robot robot = m_running.robot();
		robot.enableTreasureDetector(false);
		//robot.enableObstacleDetector(false);
		robot.enableMaze1Detector(true);
		robot.enableMaze2Detector(true);
		robot.enableMaze3Detector(true);
		robot.enableMazeEntranceGuideDetector(true);
		robot.enableDropAreaDetector(false);
		robot.enableOuterWallDetector(false);
	}
	
	@Override
	public boolean succeeded() {
		return m_currState == State.EntranceReached;
	}

	@Override
	public boolean failed() {
		return m_currState == State.MazeLost;
	}

}
